#include "zf_common_headfile.h"
#include "pid.h"
extern int16_t gyro_z1;

uint8_t stop_flag = 0;
volatile int16_t Sout = 0;
volatile int16_t SoutL = 0;
volatile int16_t SoutR = 0;
volatile int16_t MoutL = 0;
volatile int16_t MoutR = 0;
volatile int16_t export = 0; // Pid计算 巡线偏差 所得输出
volatile int16_t speed_val = 0;
volatile int16_t Gyroscope_Speed=0;//陀螺仪加速度pid输出
volatile int16_t Barrier_Speed=0;

float C_kp,C_ki,C_kd;
float S_kp,S_ki,S_kd;

int16_t duty1 = 826;
int16_t duty2 = 830;
int16_t duty3 = 610;
int16_t duty4 = 610;

/*LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL
@函数名称：float constrain_float(float amt, float low, float high)
@功能说明：限幅函数
@参数说明：amt：参数 low：最低值 high：最高值
@函数返回：无
@修改时间：2022/02/24
@调用方法：constrain_float(pid->integrator, -pid->imax, pid->imax);
@备    注：
QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ*/
float constrain_float(float amt, float low, float high)
{
    return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

// pid参数初始化函数
void PidInit(pid_param_t *pid)
{
    pid->kp = 0;
    pid->ki = 0;
    pid->kd = 0;
    pid->imax = 0;
    pid->out_p = 0;
    pid->out_i = 0;
    pid->out_d = 0;
    pid->out = 0;
    pid->integrator = 0;
    pid->last_error = 0;
    pid->last_derivative = 0;
    pid->last_t = 0;
}

/*LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL
@函数名称：float constrain_float(float amt, float low, float high)
@功能说明：pid位置式控制器输出
@参数说明：
@函数返回：
@修改时间：2022/02/24
@调用方法：
@备    注：
QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ*/
float PidLocCtrl(pid_param_t *pid, float error)
{
    /* 累积误差 */
    pid->integrator += error;

    /* 误差限幅 */
    constrain_float(pid->integrator, -pid->imax, pid->imax);

    pid->out_p = pid->kp * error;
    pid->out_i = pid->ki * pid->integrator;
    pid->out_d = pid->kd * (error - pid->last_error);

    pid->last_error = error;

    pid->out = pid->out_p + pid->out_i + pid->out_d;

    return pid->out;
}

float PdLocCtrl(pd_param_t *pd_Speed,pd_param_t *pd_Cascade, float error)
{
    pd_Speed->out_p=error*pd_Speed->kp;
    pd_Speed->out_d=(error-pd_Speed->last_error)*pd_Speed->kd;
    pd_Speed->last_error=error;

    pd_Cascade->out_p=abs(error)*error*pd_Cascade->kp;
    pd_Cascade->out_d=Gyroscope.AngleSpeed*pd_Cascade->kd;

    pd_Speed->out=pd_Speed->out_p+pd_Speed->out_d+pd_Cascade->out_p+pd_Cascade->out_d;
    return pd_Speed->out;
}

float PdLocCtrl_Side(pd_param_t *pd_Speed, float error)
{
    pd_Speed->out_p=error*pd_Speed->kp;
    pd_Speed->out_d=(error-pd_Speed->last_error)*pd_Speed->kd;
    pd_Speed->last_error=error;

    pd_Speed->out=pd_Speed->out_p+pd_Speed->out_d;
    return pd_Speed->out;
}

/*LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL
@函数名称：PidIncCtrl(pid_param_t * pid, float error)
@功能说明：增量式pid控制
@参数说明：
@函数返回：
@修改时间：2022/02/24
@调用方法：
@备    注：
QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ*/
float PidIncCtrl(pid_param_t *pid, float error)
{

    pid->out_p = pid->kp * (error - pid->last_error);
    pid->out_i = pid->ki * error;
    pid->out_d = pid->kd * ((error - pid->last_error) - pid->last_derivative);

    pid->last_derivative = error - pid->last_error;
    pid->last_error = error;

    pid->out += pid->out_p + pid->out_i + pid->out_d;

    return pid->out;
}

/*LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL
@函数名称：PidCasCtrl(pid_param_t * pid, float error)
@功能说明：串级式pid控制（转向环）
@参数说明：
@函数返回：
@修改时间：2024/01/09
@调用方法：
@备    注：
QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ*/
float PidCasCtrl(pid_param_t *external_pid, pid_param_t *interior_pid, float error)
{

    external_pid->out_p = external_pid->kp * error;

    external_pid->out_d = external_pid->kd * gyro_z1;

    external_pid->out = external_pid->out_p + external_pid->out_d;

    external_pid->last_error = error;

    error = external_pid->out - gyro_z1;

    interior_pid->out_p = interior_pid->kp * error;

    interior_pid->out_d = interior_pid->kd * external_pid->last_error;

    interior_pid->out = interior_pid->out_p + interior_pid->out_d;

    return interior_pid->out;
}

/*
 * *******************************************************
 * KP -->  比例      调硬度  越大越硬
 * KI -->  积分
 * KD -->
 *********************************************************
 */

void Speed_Pid_Init(pid_param_t *pid) // 速度
{
    pid->kp = Set_Pid.Sp_kp;
    pid->ki = Set_Pid.Sp_ki;
    pid->kd = 0;
    pid->imax = 2.5;
    pid->out_p = 0;
    pid->out_i = 0;
    pid->out_d = 0;
    pid->out = 0;
    pid->integrator = 0;
    pid->last_error = 0;
    pid->last_derivative = 0;
    pid->last_t = 0;
}

void Cascade_Pd_Init(pd_param_t *pid) // 转向
{
    pid->kp = Set_Pid.C_kp;
    pid->kd = Set_Pid.C_kd;
    pid->out_p = 0;
    pid->out_d = 0;
    pid->out = 0;
    pid->last_error = 0;
}

void Side_Cascade_Pd_Init(pd_param_t *pid) // 转向
{
    pid->kp = Set_Pid.Cc_kp;
    pid->kd = Set_Pid.Cc_kd;
    pid->out_p = 0;
    pid->out_d = 0;
    pid->out = 0;
    pid->last_error = 0;
}

void Speed_Pd_Init(pd_param_t *pid) // 转向
{
    pid->kp = Set_Pid.S_kp;
    pid->kd = Set_Pid.S_kd;
    pid->out_p = 0;
    pid->out_d = 0;
    pid->out = 0;
    pid->last_error = 0;
}

void My_Pid_Init()
{
    Cascade_Pd_Init(&Cascade_Pd);         // 差速转向err*err
    Speed_Pd_Init(&Speed_Pd);             // 差速转向err
    Side_Cascade_Pd_Init(&Side_Cascade_Pd);
    Speed_Pid_Init(&Speed_Pid);           // 行进速度
}

int16_t Sideout=0;
int16_t Side_init=710;
float ssl_kp=0.0,err_kp=0.0;

void Calculat_Out()
{
    var_usr();
    float lessdown = 0.0;

    ssl_kp=1.5;
    err_kp=1.5;
    if(Island.Island_State >0&&Island.Island_State <4)
    {
        err_kp+=0.3;
    }

//    if(Err>0)
//    {
//        err_kp-=0.1;
//        ssl_kp-=0.3;
//    }

    Target_Speed.Target_Speed_Out=Target_Speed.Straight_Target_Speed-(LCDH-LonWCol.Search_Stop_Line)*ssl_kp-abs(Err)*err_kp;

    if(LonWCol.Straight_Flag)
    {
        Target_Speed.Target_Speed_Out+=40;
    }

    if(Barrier.Barrier_Flag)
    {
        Target_Speed.Target_Speed_Out-=30;
        sErr+=15;
    }

    speed_val = (int16_t)PidLocCtrl(&Speed_Pid, Target_Speed.Target_Speed_Out - encoder_data_quaddec); // 位置式PID计算，目标-编码器 输出速度偏差

    export = PdLocCtrl(&Speed_Pd,&Cascade_Pd,Err);

    Sideout = (int16_t)PdLocCtrl_Side(&Side_Cascade_Pd,sErr);

    SoutL = Target_Speed.Left_Baseline_Speed + speed_val;  // 加到原来的基准速度(死区)上，此法（固定的基准速度）可能会受电压的干扰，测试用，可改用他法
    SoutR = Target_Speed.Right_Baseline_Speed + speed_val; // 根据实际情况确定目标值，与初始设定的两电机PWM值，实际可能需要再微调

    SoutL -= export;
    SoutR += export;

//    Sideout=Err*Side_Cascade_Pd.kp;


    lessdown = abs(Err);
    lessdown = func_limit_ab(lessdown, 0, 20);

    duty1 = Target_Speed.Down_Target_Speed_Out - Err * DOWN_ERR_SCALE_FACTOR - lessdown;
    duty2 = Target_Speed.Down_Target_Speed_Out + Err * DOWN_ERR_SCALE_FACTOR - lessdown;

//    duty1 = func_limit_ab(duty1, Target_Speed.Down_Target_Speed_Out-20, Target_Speed.Down_Target_Speed_Out+20);
//    duty2 = func_limit_ab(duty2, Target_Speed.Down_Target_Speed_Out-20, Target_Speed.Down_Target_Speed_Out+20);

    BLDC(SoutR , duty1, duty2/*+25*/, SoutL); // 将计算完的数据加到电机上执行
    BLDE_Side(Side_init-Sideout,Side_init+Sideout);
}
